//------------------------------------------------------------------------------
// Wander Service for LEGO NXT V2
//
// This solution is for the LEGO NXT using the V2 services.
// It does NOT use generic contracts, but instead connects
// directly to LEGO NXT V2 services. You must have installed
// the V1.5 Refresh to have these services available.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using wanderlegov2 = ProMRDS.Robotics.WanderLegoV2;

using W3C.Soap;

// References to the required services
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using sonarsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.SonarSensor.Proxy;
using touchsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.TouchSensor.Proxy;


namespace ProMRDS.Robotics.WanderLegoV2
{
    
    /// <summary>
    /// Implementation class for WanderLegoV2
    /// </summary>
    [DisplayName("WanderLegoV2")]
    [Description("The Wander Service for the LEGO V2")]
    [Contract(Contract.Identifier)]
    public class WanderLegoV2Service : DsspServiceBase
    {
        #region constants

        const string _platform = "LEGO NXT V2";
        const float drivePower = 0.5f;  // Power driving forward
        const float rotatePower = 0.25f; // Power during rotation
        const float veerPower = 0.6f;   // Power during veering away
        const float rotateAngle = 45.0f; // Angle for Left turns
        const int driveTime = 1200;     // Time to drive forward (millisec)
        const int rotateTime = 920;     // Time to rotate

        const float backupPower = 0.5f; // Power while backing up
        const int backupTime = 1000;    // To to back up on physical contact

        #endregion

        /// <summary>
        /// _state
        /// </summary>
        private WanderLegoV2State _state = new WanderLegoV2State();

        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/wanderlegov2", AllowMultipleInstances=false)]
        private WanderLegoV2Operations _mainPort = new WanderLegoV2Operations();

        #region partners

        // Partner: NxtDrive, Contract: http://schemas.microsoft.com/robotics/2006/05/drive.html
        [Partner("NxtDrive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        drive.DriveOperations _nxtDrivePort = new drive.DriveOperations();

        // Partner: NxtUltrasonicSensor, Contract: http://schemas.microsoft.com/robotics/2007/07/lego/nxt/sonarsensor.html
        [Partner("NxtUltrasonicSensor", Contract = sonarsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        sonarsensor.UltrasonicSensorOperations _nxtUltrasonicSensorPort = new sonarsensor.UltrasonicSensorOperations();
        sonarsensor.UltrasonicSensorOperations _nxtUltrasonicSensorNotify = new sonarsensor.UltrasonicSensorOperations();

        // Partner: NxtTouchSensor, Contract: http://schemas.microsoft.com/robotics/2007/07/lego/nxt/touchsensor.html
        [Partner("NxtTouchSensor", Contract = touchsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        touchsensor.TouchSensorOperations _nxtTouchSensorPort = new touchsensor.TouchSensorOperations();
        touchsensor.TouchSensorOperations _nxtTouchSensorNotify = new touchsensor.TouchSensorOperations();

        #endregion

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public WanderLegoV2Service(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
			base.Start();
			// Add service specific initialization here.

            // Tell the user which version of the service this is
            LogInfo(LogGroups.Console, "WANDER - Built for " + _platform);

            // Subscribe to partners
            _nxtUltrasonicSensorPort.Subscribe(_nxtUltrasonicSensorNotify);
            _nxtTouchSensorPort.Subscribe(_nxtTouchSensorNotify);

            // Add notifications to the main interleave
            base.MainPortInterleave.CombineWith(
                new Interleave(
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<sonarsensor.SonarSensorUpdate>(true, _nxtUltrasonicSensorNotify, NxtUltrasonicSensorUpdateHandler),
                        Arbiter.ReceiveWithIterator<touchsensor.TouchSensorUpdate>(true, _nxtTouchSensorNotify, NxtTouchSensorUpdateHandler)
                    ),
                    new ConcurrentReceiverGroup(
                    )
                )
            );

            /*
            // Code to explicitly get a sonar range
            yield return Activate(Arbiter.Choice(
                _nxtUltrasonicSensorPort.Get(),
                delegate(sonarsensor.SonarSensorState s)
                {
                    // Do something with the sonar reading here
                    LogInfo(s.Distance);
                },
                delegate(Fault f)
                {
                    // Something went wrong!
                }
            ));
             */

        }


        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        IEnumerator<ITask> NxtUltrasonicSensorUpdateHandler(sonarsensor.SonarSensorUpdate message)
        {
            if (message.Body.Distance < 35)
            {
                LogInfo(LogGroups.Console, "Turn Left");
                drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
                request.Power = rotatePower;
                request.Degrees = (double)rotateAngle;
                _nxtDrivePort.RotateDegrees(request);
            }
            else if (message.Body.Distance < 60)
            {
                LogInfo(LogGroups.Console, "Veer Left");
                drive.SetDrivePowerRequest powerRequest = new drive.SetDrivePowerRequest();
                powerRequest.RightWheelPower = veerPower;
                powerRequest.LeftWheelPower = veerPower/2;
                _nxtDrivePort.SetDrivePower(powerRequest);
            }
            else
            {
                drive.SetDrivePowerRequest powerRequest = new drive.SetDrivePowerRequest();
                powerRequest.RightWheelPower = drivePower;
                powerRequest.LeftWheelPower = drivePower;
                _nxtDrivePort.SetDrivePower(powerRequest);
            }

            yield break;
            
        }

        IEnumerator<ITask> NxtTouchSensorUpdateHandler(touchsensor.TouchSensorUpdate message)
        {
            if (message.Body.TouchSensorOn)
            {
                LogInfo(LogGroups.Console, "Bump!");

                // Back up to get away from the obstacle
                _nxtDrivePort.SetDrivePower(-backupPower, -backupPower);
                LogInfo(LogGroups.Console, ">>> Backing Up");
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(backupTime),
                    delegate(DateTime timeout) { });
                _nxtDrivePort.SetDrivePower(0, 0);

            }

            yield break;
        }

    }
}
